/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Timer;

/**
 * Used for performing kicks during autonomous on a seperate thread.
 * This will allow us to drive around while the kicker is being reloaded.
 * @author Administrator
 */
public class AutoKickerThread extends Thread {

    private RobotMain mRobot997;
    private boolean autoPerformKick = false;
    private boolean autoPerformSoftKick = false;
    private boolean readyToKick = true; // Used to prevent race conditions.
    private boolean autoMakeReadyForKick = false;
    private boolean autoMakeReadyForSoftKick = false;

    /**
     * Constructs the AutoKicker Thread.
     * @param robot
     */
    public AutoKickerThread(RobotMain robot) {
        mRobot997 = robot;
    }
    /**
     * Waits for the main thread to signal for a kick and then performs the
     * kick of the requested type.
     */
    public void run() {
        while (mRobot997.isAutonomous() && mRobot997.isEnabled()) {
            // DO NOT PUT ANY  CODE THAT TALKS TO THE MOTORS IN THIS THREAD!!!
            // All motor code should be in the main thread.
            if(autoPerformKick)
            {
                mRobot997.performKick();
                autoPerformKick = false;
                readyToKick = true;
            }
            if(autoPerformSoftKick)
            {
                mRobot997.performSoftKick();
                autoPerformSoftKick = false;
                readyToKick = true;
            }
            if(autoMakeReadyForKick){
                mRobot997.makeReadyForKick();
                autoMakeReadyForKick = false;
                readyToKick = true;
            }
            if(autoMakeReadyForSoftKick){
                mRobot997.makeReadyForSoftKick();
                autoMakeReadyForSoftKick = false;
                readyToKick = true;
            }
            Thread.yield();
            Timer.delay(0.0005);
        }
    }

    /**
     * Signals the kicker thread to perform a big kick.
     * Delays afterwards to make sure the kick occurs, but
     * only for the kick and not for the reload.
     * This should only be called by the main thread.
     */
    public void autoMakeReadyForKick()
    {
        while(!readyToKick){
            Thread.yield();
            Timer.delay(0.0005);
        }
        autoMakeReadyForKick = true;
        readyToKick = false;
    }
    public void autoMakeReadyForSoftKick()
    {
        while(!readyToKick){
            Thread.yield();
            Timer.delay(0.0005);
        }
        autoMakeReadyForSoftKick = true;
        readyToKick = false;
    }

    public void autoKick(){
        // Waiting here will prevent race conditions.
        while(!readyToKick){
            Thread.yield();
            Timer.delay(0.0005);
        }
        autoPerformKick = true;
        readyToKick = false;
        Timer.delay(.5);
    }

    /**
     * Signals the kicker thread to perform a soft kick.
     * Delays afterwards to make sure the kick occurs, but
     * only for the kick and not for the reload.
     * This should only be called by the main thread.
     */
    public void autoSoftKick(){
        // Waiting here will prevent race conditions.
        while(!readyToKick){
            Thread.yield();
            Timer.delay(0.0005);
        }
        autoPerformSoftKick = true;
        readyToKick = false;
        Timer.delay(2.5);
    }
}
